机器人外部工具功能应用
| 修订日期 | 修订版本 | 修订内容 | 修订人 |
|---|---|---|---|
| 20240416 | v0.1 | 初始化外部工具应用文档 | 袁紫衣 |
| 20240813 | v0.2 | 增加设置参考轨迹函数说明 | 赵锦强 |
该文档将介绍外部工具的相关概念以及应用方法，主要包括以下几个方面：
- 什么是外部工具
- 外部工具适用于那些场景
- 怎么应用外部工具
1. 外部工具功能说明
外部工具（Remote TCP）即机械臂末端安装工件，而工具安装在远端，与传统的末端工具不同。
当工具不易安装在机器人末端或工具体积大、重量重，或者希望一台机械臂完成工件加工和搬运时，我们需要采用外部工具功能来完成这个任务，例如：
- 将零件移动到喷漆枪前进行喷漆
- 对零件进行抛光，然后放置到上料托盘上
- 自动缝纫功能
- 挡风玻璃涂胶等
1.1 坐标系说明
如下图所示，左边为正常工具，工具安装的机械臂的末端；右边为外部工具，工具安装在远端。
坐标系说明：
- : 机械臂末端特征坐标系
- : 远端特征坐标系
- : 法兰坐标系
- : 基坐标系
- A: 目标轨迹上的任意一点，通常相对于 描述
1.2 末端工具应用步骤
外部工具的应用流程主要包括以下几个步骤：
定义（标定）外部工具坐标系 在 base 坐标系的描述
定义（标定）工件坐标系 在法兰坐标系的描述
使能外部工具
设置外部工具坐标系
设置工件坐标系
定义路点
路点基于工件坐标系描述
定义运动属性
正常设置
添加运动
在每段路径上增加工具和工件属性；若未定义，则默认使用model中的工具和工件属性，若在运动中需要更换工具或工件，则也需要在更换任务时更改model中的工具和工件属性来实现
开始运动
2. 接口说明
1. 定义（标定）外部工具坐标系
定义（标定）外部工具坐标系 在 base 坐标系的描述
/**
* @brief 标定远端特征坐标系的位置和姿态在基坐标系的描述
* @param method: 姿态标定方法, 具体含义见 CoordCalibMethod 结构体
* @param F_b_end_calib_point: 标定点 (末端坐标系) 在基坐标系下的位姿( = 3), 其顺序和标定类型的描述一致
* 下面以标定类型为 O_X_XY 为例说明参数的含义:
* 1) 运动到远端坐标系原点，记录此时末端坐标系在基坐标下的位姿，为 F_b_end_calib_point[0] 的输入;
* 2) 沿远端特征坐标系X轴运动一定的距离，记录此时末端坐标系在基坐标下的位姿，为 F_b_end_calib_point[1] 的输入;
* 3) 沿远端特征坐标系XY平面运动一定的距离，记录此时末端坐标系在基坐标下的位姿，为 F_b_end_calib_point[2] 的输入;
* @param F_b_remote: 远端坐标系位姿在基坐标系的描述
* @return: 返回值 < 0, 表示标定失败
*/
ARAL_API_BASIC(1.0) int calibRemoteFeatureFrameInBase(const CoordCalibMethod& method, const std::vector<RLPose>& F_b_end_calib_point, RLPose& F_b_remote) = 0;
2. 定义（标定）工件坐标系
定义（标定）工件坐标系 在法兰坐标系的描述
/**
* @brief 标定机器人末端特征坐标系的姿态在法兰坐标系的描述
* @param method: 姿态标定方法, 具体含义见 CoordCalibMethod 结构体
* @param F_b_f_calib_point: 法兰在基坐标系的位姿( = 3), 其顺序和标定类型的描述一致(保持法兰姿态不变)
* 下面以标定类型为 O_X_XY 为例说明参数的含义:
* 1) 运动到末端特征坐标系原点，记录此时法兰在基坐标下的位姿，为 F_b_f_calib_point[0] 的输入;
* 2) 保持末端特征坐标系姿态不动，沿末端特征坐标系X轴运动一定的距离，记录此时法兰在基坐标下的位姿，为 F_b_f_calib_point[1] 的输入;
* 3) 保持末端特征坐标系姿态不动，沿末端特征坐标系XY平面运动一定的距离，记录此时法兰在基坐标下的位姿，为 F_b_f_calib_point[2] 的输入;
* @param R_f_end: 末端特征坐标系的姿态在法兰坐标系的描述
* @return: 返回值 < 0, 表示标定失败
*/
ARAL_API_BASIC(1.0) int calibEndFeatureOriInFlange(const CoordCalibMethod& method, const std::vector<RLPose>& F_b_f_calib_point, Array3d& R_f_end) = 0;
/**
* @brief 标定末端特征坐标系原点的位置在法兰坐标系的描述
* @param F_b_f_calib_point: 法兰在基坐标下的位姿( >= 4), 保持末端特征坐标系原点不动，让机械臂运动到四个及以上的构型，记录此时法兰在基坐标下的位姿，即为此输入.
* @param P_f_end: 末端特征坐标系的位置在法兰坐标系的描述
* @param error: 标定后最大的位置误差
* @return: 返回值 < 0, 表示标定失败
*/
ARAL_API_BASIC(1.0) int calibEndFeaturePosInFlange(const std::vector<RLPose>& F_b_f_calib_point, Array3d& P_f_end, double& error) = 0;
3. 使能外部工具
/**
* @brief 是否使能外部（远端）工具
* 1. 先调用该接口，再设置工具和工件的坐标系
* 2. 当状态发生变化时，重新设置工具和工件的坐标系
* @param enable 为 true，表示使能外部工具
* @return if < 0, 表示使能失败
*/
ARAL_API_COMMON(1.0) int mdlEnableRemoteTool(bool enable) = 0;
4. 设置外部工具坐标系
/**
* @brief 设置工具坐标系的位姿(调用此函数会触发规划初始点状态的更新)
* 1. 如果是末端工具,则为 tcp 相对于法兰坐标系的位姿
* 2. 如果是远端工具,则为 tcp 相对于基坐标系的位姿, 需要先调用 mdlEnableRemoteTool 来使能远端工具
* @param pose: 位置和姿态数组信息
* @return if < 0, 表示设置失败
*/
ARAL_API_COMMON(1.0) int mdlSetToolPose(const RLPose& pose) = 0;
5. 设置工件坐标系
/**
* @brief 设置工件坐标系的位姿
* 1. 如果是末端工具，则为 工件坐标系 相对于 基坐标系 的位姿
* 2. 如果是远端工具，则为 工件坐标系 相对于 法兰坐标系 的位姿, 需要先调用 mdlEnableRemoteTool 来使能远端工具
* @param pose: 位置和姿态数组信息
* @return if < 0, 表示设置失败
*/
ARAL_API_COMMON(1.0) int mdlSetWorkpiecePose(const RLPose& pose) = 0;
6.添加运动
/**
* @brief 向规划器中添加直线运动(关节空间或笛卡尔空间)
* @param point: 末端目标位姿或关节构型
* @param path_property: 直线路径的几何属性
* @param move_property: 直线路径的运动属性
* @param tool_workpiece: 工具和工件信息
* @return: 返回值 < 0 表示添加失败, 具体定义查询return_value_definition.hpp
*/
ARAL_API_COMMON(1.0) int tpAddPositionLine(const PathPoint& point, const PathProperty& path_property, const MoveProperty& move_property, const ToolWorkpiece& tool_workpiece) = 0;
/**
* @brief 向规划器中添加多点运动路径, 具体的类型参考PathProperty.CurveProperty.type(关节空间或笛卡尔空间)
* @param points: 描述路径几何信息的路径点(id指的第一段路径的id, 后面的id依次递增)
* @param path_property: 路径对应的几何属性
* @param move_property: 路径对应的运动属性
* @param tool_workpiece: 工具和工件信息
* @return: 返回值 < 0 表示添加失败, 具体定义查询return_value_definition.hpp
*/
ARAL_API_COMMON(1.0) int tpAddPoints(const std::vector<PathPoint>& points, const PathProperty& path_property, const MoveProperty& move_property, const ToolWorkpiece& tool_workpiece) = 0;
struct ToolWorkpiece
{
int type{-1}; // -1: 未定义(使用设置到model中的工具和工件); 0: 正常工具; 1: 外部工具
RLPose tool; // 工具位姿: 如果是末端工具,则为 tcp 相对于法兰坐标系的位姿; 如果是远端工具,则为 tcp 相对于基坐标系的位姿
RLPose workpiece; // 工件位姿: 如果是末端工具，则为 工件坐标系 相对于 基坐标系 的位姿; 如果是远端工具，则为 工件坐标系 相对于 法兰坐标系 的位姿
RLInertia payload; // 安装在法兰末端的工具或者工件的有效载荷
};// 工具工件信息
3. 应用示例
SUITE(SUITE_ARAL_KD_ESTENSION_REMOTE_TCP_FLOW)
{
interface::RLPose F_b_t{0.202868, -0.297574, 0.48041, 3.05433, -3.65923e-06, 0.942478};
interface::RLPose F_f_w{0, 0.005, 0.01, 0, 0, 0};
bool enable_remote_tcp = true;
TEST_FIXTURE(AuboRobotInterface, testRemoteTCPFlow)
{
//0. 初始化机械臂
Setup("aubo_i5");
//1. 定义（标定）外部工具坐标系 在 base 坐标系的描述
interface::RLPose T_b_t = F_b_t;
//2. 定义（标定）工件坐标系 在法兰坐标系的描述
interface::RLPose T_f_w = F_f_w;
//3. 使能外部工具
robot->mdlEnableRemoteTool(enable_remote_tcp);
//4. 设置外部工具坐标系
robot->mdlSetToolPose(T_b_t);
//5. 设置工件坐标系
robot->mdlSetWorkpiecePose(T_f_w);
//6. 定义路点
std::vector<std::vector<double>> keypoint = { { 0, 0, 0, 0, 0, 0}, // 相对于工件坐标系描述
{0.3000, 0, 0, 0, 0, 0},
{0.3000, 0.2, 0, 0, 0, 0} };
int N = keypoint.size();
std::vector<interface::RLPose> path_points(N);
std::vector<interface::PathPoint> points(N);
for(int i = 0; i < N; i++)
{
memcpy(path_points[i].data(), keypoint[i].data(), sizeof (double) * dof);
points[i] = generatePathPoint(interface::DescribeSpace::CARTESIAN, i+1, {}, path_points[i]);
}
//7. 定义运动属性
std::vector<double> maxV = {0.5, 3.0};
std::vector<double> maxA = {4.0, 15.0};
std::vector<double> maxJ = {32.0, 120.0};
interface::PathProperty path_property = generatePathProperty({0, 0}, ARAL_TP_DEFAULT_BOW_HIGH_ERR, interface::DescribeSpace::CARTESIAN);
path_property.blend_radius = {-1, -1};
interface::MoveProperty move_property = generateMoveProperty(interface::MoveMode::POSITION, 0, maxV, maxA, maxJ);
//8. 添加运动
t_w.tool = T_b_t;
interface::RLJntArray joint_res;
interface::IKConfigInfo config;
config.goal = T_b_t;
int ret = robot->kdCalInversePosition(config, true, joint_res);
CHECK(ret >= 0);
initiatePlanner(robot, joint_res, t_w.tool); // 将外部工具坐标系原点求逆解得到的关节角作为起始点
for(int i = 0; i < N; i++)
{
ret = robot->tpAddPositionLine(points[i], path_property, move_property, t_w);
CHECK(ret >= 0);
}
//9. 开始运动
std::vector<interface::TrajectoryPoint> plan_points;
std::vector<interface::RLJntArray> command_pos, command_vel, command_acc;
std::vector<double> t;
ret = generateTrajectoryPoints(robot, path_points[0], 0.005, plan_points, command_pos, command_vel, command_acc, t);
CHECK(ret >= 0);
plot(plan_points, command_pos, command_vel, command_acc, t);
}
}
4. 常见的问题
4.1 ToolWorkpiece结构体和mdlSetToolPose的关系
struct ToolWorkpiece { int type{-1}; // -1: 未定义(使用设置到model中的工具和工件); 0: 正常工具; 1: 外部工具 RLPose tool; // 工具位姿: 如果是末端工具,则为 tcp 相对于法兰坐标系的位姿; 如果是远端工具,则为 tcp 相对于基坐标系的位姿 RLPose workpiece; // 工件位姿: 如果是末端工具，则为 工件坐标系 相对于 基坐标系 的位姿; 如果是远端工具，则为 工件坐标系 相对于 法兰坐标系 的位姿 RLInertia payload; // 安装在法兰末端的工具或者工件的有效载荷 };// 工具工件信息这里面有 tool, 与 mdlSetToolPose 是什么关系？ 这里面有 payload, 与 mdlSetLoadDynamicParameterInFlange 是什么关系？
以上接口含义如下：
ToolWorkpiece里的tool是运动路径的配置信息，payload 是运动路径的负载信息mdlSetToolPose设置的是机器人模型里面的工具属性信息，该属性存储在机器人模型里面mdlSetLoadDynamicParameterInFlange设置的是机器人模型里面的负载信息，该属性存储在机器人模型里面通过
mdl开头的接口设置到模型里面的参数，在进行正逆运动学和动力学计算时，将计算该部分的信息．例如kdCalInversePosition和kdCalInverseDynamics计算的是整个机械臂运动链（基座到法兰到末端）的运动学和动力学，包括通过mdl开头的接口设置到模型的负载和末端信息．以
rs和tp开头接口的ToolWorkpiece信息，不会设置到 model 模块，只会影响该段轨迹的计算
4.2 使能外部工具之后TCP标定和工件坐标系标定问题
使能外部工具之后如何进行TCP标定和工件坐标系标定?
使能外部工具之后，标定坐标系的方法和目前标定坐标系的方法是一致的．采用末端工具进行坐标系标定：
标定外部工具坐标系
类似于标定用户坐标系，采用一样的标定方法，确定该坐标系相对于基坐标系的描述．
标定末端工件坐标系
类似于标定末端坐标系，采用四点法来标定，确定末端坐标系相对于法兰坐标系的描述
采用末端工具标定结束后，再使能外部工具，设置坐标系．
4.3 取点及设置参考轨迹说明
tpUpdateCycle函数可以拿到每段路径的ToolWorkpiece信息
struct TrajectoryPoint { MoveMode mode; // 轨迹点对应的运动模式(0: 位置模式; 1: 速度模式) double time; // 轨迹点对应的路径完成时间, 取值[0, duration] PathPoint point; // 轨迹点对应的路径描述点 RLTwist xd, xdd; // 轨迹点对应的笛卡尔空间速度 加速度 RLJntArray qd, qdd; // 轨迹点对应的关节空间速度 加速度 RLJntArray effort; // 轨迹点对应的关节电流 RLWrench wrench; // 轨迹点对应的TCP处的力/力矩 ToolWorkpiece t_w; // 轨迹点上的工具工件信息(和路径属性对应) };// 轨迹（插值）点结构体0.39版本以后的版本, 设置参考轨迹1接口废弃，使用设置参考轨迹２，接口说明如下:
/**
* @brief 设置参考轨迹2(0.39版本以后升级的版本)
* @param has_ref_traj: 是否有参考轨迹: 1) false, 表示没有参数轨迹, 用于力控拖动的场景; TrajectoryPoint 参数无需设置
* 2) true, 表示有参考轨迹(一般由规划算法 tpUpdateCycle 函数生成). 在外部轴/外部工具的同步控制应用中, 用户需要显式更新 TrajectoryPoint 结构体中的 ToolWorkpiece 属性
* @param point: 一般是规划输出的轨迹点
* @return if < 0, 则表示设置失败
*/
ARAL_API_COMMON(1.0) int rsSetReferenceTrajectory(const bool& has_ref_traj, const TrajectoryPoint& point = TrajectoryPoint()) = 0;